/**=============================================================================
@file
    qhblas_hvx_h_vector_dot_ab.c

@brief
    HVX function that calculates dot product of two input 16-bit vectors.

    Function prototype
        int32_t qhblas_hvx_h_vector_dot_ab(int8_t *input_1, int8_t *input_2,
                                            int16_t *output, uint32_t size);

Copyright (c) 2020 Qualcomm Technologies Incorporated.
All Rights Reserved. Qualcomm Proprietary and Confidential.
=============================================================================**/

#include <stddef.h>
#include "hvx_internal.h"

#define BLOCK_SIZE       (8*1024/VLEN)  // vector chunks
#define L2FETCH_AHEAD    (BLOCK_SIZE)

int32_t qhblas_hvx_uw_vector_dot_aub(
    uint8_t *restrict input_1,
    uint8_t *restrict input_2,
    uint32_t *restrict output,
    uint32_t size)
{
    if ((input_1 == NULL) || (input_2 == NULL)
        || (output == NULL) || (size == 0))
    {
        return -1;
    }

    HVX_Vector *iptr1 = (HVX_Vector *) input_1;
    HVX_Vector *iptr2 = (HVX_Vector *) input_2;
    HVX_Vector sline1p, sline1c, sline1, sline2p, sline2c, sline2;
    HVX_Vector sout, zero;
    HVX_VectorPair dout, pv_tmp;
    uint32_t __attribute__((aligned(VLEN))) tmp_buf[64];
    int32_t block, l2fetch_block;
    int32_t leftover = size & (VLEN-1);
    int32_t vectors_in_rounddown = size / VLEN;
    int32_t leftover_size = leftover * sizeof(int8_t);

    sline1p = *iptr1++;
    sline2p = *iptr2++;
    dout = Q6_W_vzero();
    zero = Q6_V_vzero();

    for (int32_t i = vectors_in_rounddown - 1; i > 0; i -= BLOCK_SIZE)
    {
        block = Q6_R_min_RR(i, BLOCK_SIZE);
        l2fetch_block = Q6_R_min_RR(i - L2FETCH_AHEAD, BLOCK_SIZE);

        if (l2fetch_block > 0)
        {
            l2fetch(iptr1 + L2FETCH_AHEAD, VLEN, VLEN, l2fetch_block, 0);
            l2fetch(iptr2 + L2FETCH_AHEAD, VLEN, VLEN, l2fetch_block, 0);
        }

        for (int32_t j = 0; j < block; ++j)
        {
            sline1c = *iptr1++;
            sline2c = *iptr2++;
            sline1 = Q6_V_valign_VVR(sline1c, sline1p, (size_t) input_1);
            sline2 = Q6_V_valign_VVR(sline2c, sline2p, (size_t) input_2);
            // dout = Q6_Wuh_vmpyacc_WuhVubVub(dout, sline1, sline2);
            pv_tmp = Q6_Wuh_vmpy_VubVub(sline1, sline2);
            dout = Q6_Ww_vaddacc_WwVuhVuh(dout, Q6_V_hi_W(pv_tmp), Q6_V_lo_W(pv_tmp));

            sline1p = sline1c;
            sline2p = sline2c;
        }
    }

    // Unroll once to avoid out-of-boundary load.
    if (vectors_in_rounddown > 0)
    {
        sline1c = is_aligned(iptr1, VLEN) && leftover == 0 ? sline1p : *iptr1++;
        sline2c = is_aligned(iptr2, VLEN) && leftover == 0 ? sline2p : *iptr2++;
        sline1 = Q6_V_valign_VVR(sline1c, sline1p, (size_t) input_1);
        sline2 = Q6_V_valign_VVR(sline2c, sline2p, (size_t) input_2);

        // dout = Q6_Wuh_vmpyacc_WuhVubVub(dout, sline1, sline2);
        pv_tmp = Q6_Wuh_vmpy_VubVub(sline1, sline2);
        dout = Q6_Ww_vaddacc_WwVuhVuh(dout, Q6_V_hi_W(pv_tmp), Q6_V_lo_W(pv_tmp));

        sline1p = sline1c;
        sline2p = sline2c;
    }

    // Handle leftover elements.
    if (leftover > 0)
    {
        sline1c = (is_in_one_chunk(iptr1, leftover_size, VLEN)
                   ? sline1p
                   : *iptr1++);
        sline2c = (is_in_one_chunk(iptr2, leftover_size, VLEN)
                   ? sline2p
                   : *iptr2++);
        sline1 = Q6_V_valign_VVR(sline1c, sline1p, (size_t) input_1);
        sline2 = Q6_V_valign_VVR(sline2c, sline2p, (size_t) input_2);

        // Fill unused data with zeros.
        sline1 = Q6_V_valign_VVR(sline1, zero, leftover_size);
        sline2 = Q6_V_valign_VVR(sline2, zero, leftover_size);

        // dout = Q6_Wuh_vmpyacc_WuhVubVub(dout, sline1, sline2);
        pv_tmp = Q6_Wuh_vmpy_VubVub(sline1, sline2);
        dout = Q6_Ww_vaddacc_WwVuhVuh(dout, Q6_V_hi_W(pv_tmp), Q6_V_lo_W(pv_tmp));
    }

#if 0
    sout = Q6_Vuh_vadd_VuhVuh(Q6_V_hi_W(dout), Q6_V_lo_W(dout));
    for (int32_t i = 64; i >= 2; i >>= 1)
    {
        sout = Q6_Vuh_vadd_VuhVuh(sout, Q6_V_vlalign_VVR(sout, zero, i));
    }

    *(HVX_Vector *) tmp_buf = sout;
    *output = (tmp_buf[63] << 1);
#else
    sout = Q6_Vuw_vadd_VuwVuw_sat(Q6_V_hi_W(dout), Q6_V_lo_W(dout));
    *(HVX_Vector *) tmp_buf = sout;
    uint32_t tmp = 0;
    for (int32_t i = 0; i < 32; i++) {
        tmp += tmp_buf[i];
    }
    *output = tmp;
#endif
    return 0;
}
